#include "HexapodServo.h"
#include "HexapodServoIO.h"
#include "rtthread.h"
#include <math.h>

// angle2radian(240) = 4.1887901
// angle2radian(180) = 3.1415926
const SERVO SERVOS[20] = {
	// id, type, angle, angleOffset, second, maxAngle, maxTicks, direction
	// Leg 0
	{1,  ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, -1}, // tibi_LF
	{3,  ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, -1}, // femu_LF
    {5,  ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000,  1}, // coxa_LF
		
    // Leg 1
    {7,  ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, -1}, // tibi_LM
    {9,  ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, -1}, // femu_LM
    {11, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // coxa_LM		
		
    // Leg 2
	{13, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, -1}, // tibi_LR
	{15, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, -1}, // femu_LR
    {17, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // coxa_LR		
		
	// Leg 3
	{14, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // tibi_RR   
    {16, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // femu_RR
    {18, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // coxa_RR		
		
	// Leg 4
	{8,  ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // tibi_RM
	{10, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // femu_RM
    {12, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // coxa_RM		
		
	// Leg 5
	{2, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // tibi_RF
    {4, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // femu_RF
    {6, ServoType_BUS, 0, 0, 0.1f, 240.0f, 1000, 1}, // coxa_RF
		
	// id, type, angle, angleOffset, second, maxAngle, maxTicks, direction
    {19, ServoType_PWM, 0, 0, 0.1f, 180, 1000, -1}, // head_be
    {20, ServoType_PWM, 0, 0, 0.1f, 180, 1000, -1}, // head_af
};

// 设置位置（角度）
void setServoPosition(uint8_t servo_id, uint16_t pos, uint16_t duration) {
	// 转动位置 0~1000 对应 0~240 角度
    if (pos > 1000) {
        pos = 1000;
    }
	// 转动时间 0~30000 毫秒 
    if (duration < 0) {
        duration = 0;
    }
	// 发送数据
    serialSerroWriteCmd(servo_id, SERVO_MOVE_TIME_WRITE, pos, duration);
}

// 设置偏差
void setServoDeviation(uint8_t servo_id, int16_t value) {

    if (servo_id < 1) {
        return;
    }

    if (value < -200 || value > 200) {
        return;
    }

    uint8_t dat1 = value & 0xffff;
    serialSerroWriteCmd(servo_id, SERVO_ANGLE_OFFSET_ADJUST, dat1, int16_t_NONE);
}

void stopServo(uint8_t servo_id) {
    serialSerroWriteCmd(servo_id, SERVO_MOVE_STOP, int16_t_NONE, int16_t_NONE);
}

// 更换舵机 ID
void setServoID(uint8_t old_id, uint8_t new_id) {
    serialSerroWriteCmd(old_id, SERVO_ID_WRITE, new_id, int16_t_NONE);
}

// 读取舵机 ID  数据长度 4
bool readServoID(uint8_t servo_id, int16_t* value, int count){
	bool rnt = false;
    while (count > 0) {
        count -= 1;
        if (servo_id == 0) {
            // only one servo connected to the bus
            serialServoReadCmd(0xfe, SERVO_ID_READ);
        } else {
            serialServoReadCmd(servo_id, SERVO_ID_READ);
        }
		
        serialServoGetMsg(SERVO_ID_READ, value);
		if (value[0] != int16_t_NONE) {
			rnt = true;
			break;
		}
    }
	return rnt;
}

// 保存偏差 范围-125~125，默认值 0
void saveServoDeviation(uint8_t servo_id) {
    serialSerroWriteCmd(servo_id, SERVO_ANGLE_OFFSET_WRITE, int16_t_NONE, int16_t_NONE);
}

// 读取偏差 数据长度 4
bool readServoDeviation(uint8_t servo_id, int16_t* value, int count) {
	bool rnt = false;
    while (count > 0) {
        count -= 1;
        serialServoReadCmd(servo_id, SERVO_ANGLE_OFFSET_READ);
        serialServoGetMsg(SERVO_ANGLE_OFFSET_READ, value);
		if (value[0] != int16_t_NONE) {
			rnt = true;
			break;
		}
    }
	return rnt;
}

// 设置转动角度限制
void setServoAngleLimit(uint8_t servo_id, uint8_t low, uint8_t high) {
    serialSerroWriteCmd(servo_id, SERVO_ANGLE_LIMIT_WRITE, low, high);
}

// 读取转动角度先吃
bool readServoAngleLimit(uint8_t servo_id, int16_t* value, int count) {
	bool rnt = false;
    while (count > 0) {
        count -= 1;
        serialServoReadCmd(servo_id, SERVO_ANGLE_LIMIT_READ);
        serialServoGetMsg(SERVO_ANGLE_LIMIT_READ, value);
		if (value[0] != int16_t_NONE) {
			rnt = true;
			break;
		}
    }
	return rnt;
}

// 设置输入电压限制
void setServoVinLimit(uint8_t servo_id, int16_t low, int16_t high) {
    serialSerroWriteCmd(servo_id, SERVO_VIN_LIMIT_WRITE, low, high);
}

// 读取输入电压限制
bool readServoVinLimit(uint8_t servo_id, int16_t* value, int count) {
	bool rnt = false;
    while (count > 0) {
        count -= 1;
        serialServoReadCmd(servo_id, SERVO_VIN_LIMIT_READ);
        serialServoGetMsg(SERVO_VIN_LIMIT_READ, value);
			if (value[0] != int16_t_NONE) {
				rnt = true;
				break;
			}
    }
	return rnt;
}

// 设置舵机温度限制
void setServoMaxTemp(uint8_t servo_id, int16_t m_temp) {
    serialSerroWriteCmd(servo_id, SERVO_TEMP_MAX_LIMIT_WRITE, m_temp, int16_t_NONE);
}

// 读取舵机内部最高温度限制，范围 50~100℃ 数据长度 4
bool readServoTempLimit(uint8_t servo_id, int16_t* value, int count) {
	bool rnt = false;
    while (count > 0) {
			count -= 1;
			serialServoReadCmd(servo_id, SERVO_TEMP_MAX_LIMIT_READ);
			serialServoGetMsg(SERVO_TEMP_MAX_LIMIT_READ, value);
			if (value[0] != int16_t_NONE) {
				rnt = true;
				break;
			}
    }
	return rnt;
}

// 读取舵机当前的实际角度位置值 数据长度 5
bool readServoPos(uint8_t servo_id, int16_t* value, int count) {
    bool rnt = false;
    while (count > 0) {
        count -= 1;
        serialServoReadCmd(servo_id, SERVO_POS_READ);
        serialServoGetMsg(SERVO_POS_READ, value);
		if (value[0] != int16_t_NONE) {
			rnt = true;
			break;
		}
    }
	return rnt;
}

// 读取舵机当前的实际角度位置值
bool readServoPosition(uint8_t servo_id, int16_t* value, int count) {
    return readServoPos(servo_id, value, count);
}

// 读取舵机内部当前温度 数据长度4
bool readServoTemp(uint8_t servo_id, int16_t* value, int count) {
    bool rnt = false;
    while (count > 0) {
        count -= 1;
        serialServoReadCmd(servo_id, SERVO_TEMP_READ);
        serialServoGetMsg(SERVO_TEMP_READ, value);
		if (value[0] != int16_t_NONE) {
			rnt = true;
			break;
		}
    }
	return rnt; 
}

// 读取舵机电压 数据长度 5
bool readServoVin(uint8_t servo_id, int16_t* value, int count) {
    bool rnt = false;
    while (count > 0) {
        count -= 1;
        serialServoReadCmd(servo_id, SERVO_VIN_READ);
        serialServoGetMsg(SERVO_VIN_READ, value);
		if (value[0] != int16_t_NONE) {
			rnt = true;
			break;
		}
    } 
	return rnt; 
}

// 舵机复位
void resetServoPos(uint8_t old_id) {
	// 清零偏差
    setServoDeviation(old_id, 0);
    // ms
	rt_thread_delay(20);
	// 中位
    serialSerroWriteCmd(old_id, SERVO_MOVE_TIME_WRITE, 500, 100);
}

// 舵机内部电机是否卸载掉电，范围 0 或 1，0 代表卸载掉电，此时舵机
// 无力矩输出。1 代表装载电机，此时舵机有力矩输出，默认值 0。
void writeServoTau(uint8_t servo_id, int16_t new_state) {
    serialSerroWriteCmd(servo_id, SERVO_LOAD_OR_UNLOAD_WRITE, new_state, int16_t_NONE);
}

// 读取舵机内部电机的状态
bool readServoTau(uint8_t servo_id, int16_t* value, int count) {
    bool rnt = false;
    while (count > 0) {
			count -= 1;
			serialServoReadCmd(servo_id, SERVO_LOAD_OR_UNLOAD_READ);
			serialServoGetMsg(SERVO_LOAD_OR_UNLOAD_READ, value);
			if (value[0] != int16_t_NONE) {
				rnt = true;
				break;
			}
    }
	return rnt;     
}

void setServoAngle(SERVO* servo) {
	uint8_t servo_id = servo->id; 
	uint8_t servo_type = servo->type;  
	float angle = servo->angle;
	float angleOffset = servo->angleOffset;
	float second = servo->second;
	float maxAngle = servo->maxAngle;
	uint16_t maxTicks = servo->maxTicks; 
	int8_t direction = servo->direction;

	// 舵机位置数值的最大最小值
    const uint16_t min_ticks = 0;

	// 最终的舵机真实角度
	const float half_angle = maxAngle * 0.5;
    float real_angle = direction * (angleOffset + angle);
    if (fabs(real_angle) > half_angle) {
        real_angle = direction * half_angle;
    }
		
    // 计算舵机位置数值
	const float rate = (maxTicks - min_ticks) / maxAngle;
	float f_real_ticks = (real_angle + half_angle) * rate + min_ticks;
    uint16_t real_ticks = f_real_ticks;
		
    // 限幅, 确保数值有效
    if (real_ticks > maxTicks) {
        real_ticks = maxTicks;
    }

    if (real_ticks < min_ticks) {
        real_ticks = min_ticks;
    }

    // ms -> second
    uint16_t time = second * 1000 + 0.5;
    if (servo_type == ServoType_BUS) {
        setServoPosition(servo_id, real_ticks, time);
    }
		servo->angle = real_angle;
}

void test_servo_state() {
	bool isRead = false;
	int16_t value[2] = {int16_t_NONE};
	// 读取设备 ID
    isRead = readServoID(1, value, 100);
	if (value[0] != int16_t_NONE) {
		// 舵机 ID
		uint8_t servo_id = value[0] & 0xff;
		
		// 读取位置
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoPosition(servo_id, value, 100);
		uint16_t postion = value[0] & 0xffff;
		
		// 读取温度
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoTemp(servo_id, value, 100);
		uint8_t temperature = value[0] & 0xff;		
		
		// 读取电压 毫伏
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoVin(servo_id, value, 100);
		uint16_t voltage = value[0] & 0xffff;	
		
		// 读取偏差
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoDeviation(servo_id, value, 100);
		int8_t deviation = value[0] & 0xff;
		
		// 读取角度限制 pos_min - pos_max
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoAngleLimit(servo_id, value, 100);
		uint16_t pos_max = value[0] & 0xffff;
		uint16_t pos_min = value[1] & 0xffff;	
		
		// 读取电压限制 vin_min - vin_max
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoVinLimit(servo_id, value, 100);
		uint16_t vin_max = value[0] & 0xffff;
		uint16_t vin_min = value[1] & 0xffff;	
		
		// 读取温度限制 50 - temper_max
		value[0] = int16_t_NONE; 
		value[1] = int16_t_NONE; 
		isRead = readServoTempLimit(servo_id, value, 100);
		uint8_t temper_max = value[0] & 0xff;
	}
}
